 /*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Main extends IterativeRobot {


    DriverStation DS;
    RobotDrive RD;

    static final int UNINIT_DRIVE = 0;
    static final int ARCADE_DRIVE = 1;
    static final int TANK_DRIVE = 2;

    private boolean TriggerStart = false;


    static final int JoystickCount = 3; //Change if we plan to add new joysticks

    //Drive Control
    Joystick J_Num1;
    Joystick J_Num2;
    //Ball Control
    Joystick J_Num3;


    static final int JoystickButtonNum = 16;
    boolean[] J_Num1ButtonState = new boolean[Main.JoystickButtonNum + 1];
    boolean[] J_Num2ButtonState = new boolean[Main.JoystickButtonNum + 1];
    boolean[] J_Num3ButtonState = new boolean[Main.JoystickButtonNum + 1];


    Jaguar RightMotor;
    Jaguar LeftMotor;
    Jaguar Kick;

    Victor Roller;


    int printLoop = 0;

    private int LoopsTrigger = 0;
    private int delayLoops = 0;

    private boolean delayStarted = false;

    Encoder KickEncoder;

    AxisCamera Cam;
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit()
    {
        System.out.println("Starting Team 1872 Robot Code");

        System.out.println("Starting Joysticks");

        J_Num1 = new Joystick(1);
        J_Num2 = new Joystick(2);
        J_Num3 = new Joystick(3);

        System.out.println("Button Mapping...");

        MapButtons();

        System.out.println("Joysticks Initialized");

        System.out.println("Starting Motor Jaguars");

        RightMotor = new Jaguar(1);
        LeftMotor = new Jaguar(2);

        System.out.println("Starting Robot Drive System");

        RD = new RobotDrive(RightMotor, LeftMotor);

        System.out.println("Drive System Initialized");


        System.out.println("Kicking Mechanism Starting...");

        Kick = new Jaguar(3);
        Roller = new Victor(4);

        System.out.println("Robot Finished Succesfully");

        //END INIT
    }

        void updateDashboard()
        {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++)
                    {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++)
                            {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            lowDashData.addByte(Solenoid.getAll());
        }
        lowDashData.finalizeCluster();
       
        lowDashData.commit();
    }

    private void MapButtons()
    {
        for(int i = 1; i <= Main.JoystickButtonNum; i++)
        {
            J_Num1ButtonState[i] = false;
            J_Num2ButtonState[i] = false;
            J_Num3ButtonState[i] = false;
        }

        KickEncoder = new Encoder(1,2);

        KickEncoder.start();
    }

    public void teleopInit()
    {

        System.out.println("Teleop Init... \n\n");

        //Start Init DEBUG

        System.out.println("DEBUG: Test Kicking Mechanism...");
        System.out.println("DEBUG: Jaguar at port 3 = " + Kick.getSpeed());
        //Test the kicking mechanism on init
        Kick.set(0.1);
        System.out.println("DEBUG: Jaguar at port 3 = " + Kick.getSpeed());
        Kick.set(0.0);
        System.out.println("DEBUG: Jaguar at port 3 = " + Kick.getSpeed());
        System.out.println("DEBUG: STOP");

        //End Init DEBUG
        
        System.out.println("Setting Drive Mode \n");

        RD.tankDrive(J_Num1, J_Num2); //First Arg is Left Second is Right


        AxisCamera.getInstance();

        Cam = AxisCamera.getInstance();
        Cam.writeRotation(AxisCamera.RotationT.k180);
    }

    public void disabledPeriodic()
    {
        //super.disabledPeriodic();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic()
    {
        System.out.println("Autonomous Mode...");
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic()
    {
        Watchdog.getInstance().feed();

        AxisCamera.getInstance();

        //Test Camera
        //System.out.println("DEBUG: Cam Brightness " + Cam.getBrightness());

        /*
        if(Cam.freshImage())
        {
            System.out.println("DEBUG: Image Refreshed");
        }
        else
        {
            System.out.println("DEBUG: Image Not Refreshed");
        }
        */

        //Debug Mesage
        //System.out.println("Teleop Mode...");

        RD.tankDrive(J_Num1, J_Num2);

        //Debug Message - Print the Status of the Trigger
        //System.out.println("Joystick #1 Trigger: " + J_Num1.getTrigger());

        if(J_Num1.getTrigger())
        {
            TriggerStart = true;
            if(LoopsTrigger < 10 && delayCheck())
            {
                System.out.println("DEBUG: Joystick #1 Trigger = " + J_Num1.getTrigger());
                Kick.set(1.0);
                System.out.println("DEBUG: KICK Speed = " + Kick.getSpeed());
            }
            else if(LoopsTrigger > 10 && LoopsTrigger < 10)
            {
                Kick.set(-1.0);
                System.out.println("DEBUG: Kick on Reverse");
            }
            else
            {
                Kick.set(0.0);
                startDelay();
            }
        }
        else
        {
            Kick.set(0);
            TriggerStart = false;
        }

        if(J_Num2.getTrigger())
        {
            System.out.println("DEBUG: Joystick #2 Trigger = " + J_Num2.getTrigger());
            Roller.set(1);
            System.out.println("DEBUG: Roller Speed = " + Roller.getSpeed());
        }
        else
        {
            Roller.set(0);
        }

        if(TriggerStart)
        {
            LoopsTrigger++;
            System.out.println("DEBUG: Trigger Loops = " + LoopsTrigger);
        }
        else
            LoopsTrigger = 0;

        KickMove(J_Num3.getY());

        System.out.println("DEBUG: Encoder = " + KickEncoder.getRaw());

        updateDashboard();
    }

    public boolean CheckButtonJoy3(int button)
    {
        //Debug Message
        //System.out.println("Check Button Running...");

        return false;
    }

    /**
     * Called Continuously though teleop
     */
    public void teleopContinuous() {

    }

    private void startDelay()
    {
        if(delayStarted)
            delayLoops++;
        else
            delayStarted = true;
    }

    private boolean delayCheck()
    {
        System.out.println("DEBUG: delayLoops = " + delayLoops);
        if(delayLoops > 5)
        {
            delayLoops = 0;
            return false;
        }
        else
            return true;
    }

    private void KickMove(double Y_Axis)
    {
        Kick.set(Y_Axis);
        //System.out.println("DEBUG: Y_Axis = " + Y_Axis);
        //System.out.println("DEBUG: Kick Speed = " + Kick.getSpeed());
    }
    
}
